/*
 * planner.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   planner.c
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   23 sep 2011
 * @brief  The planner component translates movement g-code commands into 
 *         blocks that represent a linear movement and places these blocks
 *         into a queue of blocks to be executed by the block handler component.
 */

#include "planner.h"

#include "yarf.h"
#include "block.h"
#include "planner_queue.h"
#include "planner_lookahead.h"
#include "block_handler.h"
#include "advance.h"
#include "scheduling/realtime_timer.h"
#include "util/math.h"
#include "util/fixed_point.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <util/atomic.h>

/**
 * The number of millimeters the machine will back up on its X axis, after
 * reaching its X min endstop during the first stage of homing.
 */
#define HOME_AXIS_CALIBRATION_MM_X  5
/**
 * The number of millimeters the machine will back up on its Y axis, after
 * reaching its Y min endstop during the first stage of homing.
 */
#define HOME_AXIS_CALIBRATION_MM_Y  5
/**
 * The number of millimeters the machine will back up on its Z axis, after
 * reaching its Z min endstop during the first stage of homing.
 */
#define HOME_AXIS_CALIBRATION_MM_Z  1

/**
 * The speed in mm/min at which the machine will move its X axis, during the
 * second stage of homing.
 */
#define HOME_AXIS_CALIBRATION_SPEED_X 300
/**
 * The speed in mm/min at which the machine will move its Y axis, during the
 * second stage of homing.
 */
#define HOME_AXIS_CALIBRATION_SPEED_Y 300
/**
 * The speed in mm/min at which the machine will move its Z axis, during the
 * second stage of homing.
 */
#define HOME_AXIS_CALIBRATION_SPEED_Z 50


/**
 * Flags indicating which axes have been homed since boot.
 */
static uint8_t axis_homed;


/**
 * The virtual position in steps of each axis. The printer will actually be at
 * this position when all pending blocks in the planning queue have been
 * executed.
 */
static steps_t virtual_position[NUM_AXES];


/**
 * The identifier number to use for the next block. The identifier numbers are
 * only used for debugging purposes.
 */
static unsigned int next_id;



/**
 * Given that the machine starts a movement at a certain entry speed and that
 * it needs to end this movement at a certain exit speed after a certain
 * distance in millimeter and number of steps, this function returns the number
 * of steps after which the machine must end accelerating and start
 * decelerating.
 *
 * This function can be used to compute the intersection point between the 
 * acceleration slope and the deceleration slop for the case where the block 
 * trapezoid has no plateau:
@verbatim
             speed ^
                   |       +
                   |      /:\
                   |     / : \
                   |    /  :  + <-- exit speed
     entry speed --|-> +   :  |
                   |   |   :  |
                   |   +---|--+
                   |       +------> intersection time
                   |____________________________
                                         time -->

 This function returns the nb of steps travelled at the intersection time. The
 easiest way to calculate this, is by using the classic formulas for uniform
 acceleration:
  V(t) = Vi + a*t
  s(t) = (Vi + Vf)*t/2
 Where Vi is the entry speed in mm/min, 
       Vf is the exit speed in mm/min,
        s is the displacement in mm and
        t is the time passed

 From the second equation, we get:
  t(s) = 2*s/(Vi+Vf)
 So
  V(s) = Vi + 2*a*s/(Vi+Vf)

 We can use this formula directly for the acceleration curve:
  Va(s) = Vi + 2*a/(Vi+Vf) * s

 And by shifting and mirroring it, we can also use it for the deceleration
 curve:
  Vd(s) = Vf + 2*a(Vi+Vf) * (l - s)
 Where l is the length in mm of the entire movement

 Now we can just solve for the intersection of Va and Vd, which gives us (after
 performing some substitutions and simplifications):
  s = (Vf² - Vi²)/(4*a) + l/2

 All that's left is to convert s (which is in mm) to the number of linear steps.
 This can simply be done by multiplying by the average number of steps per mm
 of the movement:
  result = s * b/l
         = b * [(Vf² - Vi²)/(4*a*l) + 1/2]
         = b * [(Vf + Vi)(Vf - Vi)/(4*a*l) + 1/2]
 With b the total number of steps of the entire movement.

@endverbatim
 */
static inline int32_t
intersection_steps(speed_t entry_speed, speed_t exit_speed, float distance_mm, long nb_steps)
{
  return lround(nb_steps * ( ((exit_speed+entry_speed)*(exit_speed-entry_speed))/(4*ACCELERATION*distance_mm) + 0.5 ));
}


/**
 * Returns the fixed point 16.16 representation of a (positive) floating point
 * number, if the number is less than or equal to the half of the largest
 * representable value in the fixed point representation. If it is larger than
 * half of the maximum value, the half of the maximum value is returned.
 *
 * @param ticks_fl the floating point number to convert
 * @return The fixed point 16.16 value that is closest to the value of the 
 *         given floating point number, but is less than or equal to the half
 *         of the largest representable fixed point number.
 *
 * This method is used to convert floating point timer ticks into a fixed point
 * 16.16 representation, which can safely be multiplied by 2, without causing
 * an overflow. This is because the block handler needs to multiply the current
 * timer tick value by two in order to calculate the new timer tick value.
 */
static inline fxp16u16_t
ticks_float_to_fxp16u16_safe(float ticks_fl)
{
  if (ticks_fl < fxp16u16_to_float(FXP_16U16_MAX/2)) {
    return fxp16u16_from_float(ticks_fl);
  } else {
    return FXP_16U16_MAX/2;
  }
}



/**
 * Initializes a number of basic parameters for a block, so it can be executed
 * by the block handler.
 *
 * @param b                 a pointer to the block to initialize
 * @param steps_x           the number of steps the block needs to take in the
 *                          x direction (can be negative)
 * @param steps_y           the number of steps the block needs to take in the
 *                          y direction (can be negative)
 * @param steps_z           the number of steps the block needs to take in the
 *                          z direction (can be negative)
 * @param steps_e           the number of steps the block needs to take in the
 *                          e direction (can be negative)
 * @param plateau_speed     maximum speed of the block in mm/min
 * @param collision_handler a pointer to a function to be called when one of 
 *                          the endstop is hit during execution of the block

 * After calling this method, the block is ready to be placed into the planning
 * queue, to be executed by the block handler. The entry and exit speed of the
 * block are set very conservatively to the minimum of the given plateau speed
 * and the configured \a START_SPEED. These speeds can be increased by running
 * the lookahead component after placing the block into the planning queue.
 */
static inline void
initialize_block(block_t *b, steps_t steps_x, steps_t steps_y, steps_t steps_z, steps_t steps_e, speed_t plateau_speed, void (*collision_handler)(uint8_t))
{
  /* Set the block's id */
  b->id = next_id++;

  /* Set the collision handler */
  b->collision_handler = collision_handler;

  /* The number of steps in each direction and the total number of steps */
  b->steps[X_AXIS] = steps_x;
  b->steps[Y_AXIS] = steps_y;
  b->steps[Z_AXIS] = steps_z;
  b->steps[E_AXIS] = steps_e;
  b->nb_steps = MAX(labs(b->steps[X_AXIS]), MAX(labs(b->steps[Y_AXIS]),
                  MAX(labs(b->steps[Z_AXIS]), labs(b->steps[E_AXIS]))));
  b->deceleration_start = b->nb_steps;
  b->nb_steps_completed = 0;

  /* The distance to travel in each direction in mm */
  float distance_mm[NUM_AXES];
  distance_mm[X_AXIS] = steps_x*X_MM_PER_STEP;
  distance_mm[Y_AXIS] = steps_y*Y_MM_PER_STEP;
  distance_mm[Z_AXIS] = steps_z*Z_MM_PER_STEP;
  distance_mm[E_AXIS] = steps_e*E_MM_PER_STEP;

  /* The straight-line distance (must not be zero) */
  float distance_mm_line = sqrt(
    fsquare(distance_mm[X_AXIS])+ 
    fsquare(distance_mm[Y_AXIS])+ 
    fsquare(distance_mm[Z_AXIS])+ 
    fsquare(distance_mm[E_AXIS]));
  b->distance_mm = distance_mm_line;
  b->mm_ticks_per_step_min = ((float)(distance_mm_line*RTTIMER_TICKS_PER_MIN))/b->nb_steps; // mm per step multiplied by RTTIMER_TICKS_PER_MIN

  /* The 4D direction unit vector of this move */
  float distance_mm_line_inverse = 1.0/distance_mm_line;
  b->direction[X_AXIS] = distance_mm[X_AXIS]*distance_mm_line_inverse;
  b->direction[Y_AXIS] = distance_mm[Y_AXIS]*distance_mm_line_inverse;
  b->direction[Z_AXIS] = distance_mm[Z_AXIS]*distance_mm_line_inverse;
  b->direction[E_AXIS] = distance_mm[E_AXIS]*distance_mm_line_inverse;

  /* Limit the feed rate according to the max feed rate of each axis */
  if (distance_mm[X_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(X_MAX_SPEED/b->direction[X_AXIS])));
  if (distance_mm[Y_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(Y_MAX_SPEED/b->direction[Y_AXIS])));
  if (distance_mm[Z_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(Z_MAX_SPEED/b->direction[Z_AXIS])));
  if (distance_mm[E_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(E_MAX_SPEED/b->direction[E_AXIS])));
  b->plateau_speed = plateau_speed;
  speed_t start_feed_rate = MIN(plateau_speed, START_FEED_RATE);

  /* Set the number of ticks in between steps at the plateau */
  b->plateau_timer_ticks = ticks_float_to_fxp16u16_safe(b->mm_ticks_per_step_min/plateau_speed);


  /* Calculate a conservative acceleration/deceleration trapezoid */
  plan_calculate_trapezoid(b, start_feed_rate, start_feed_rate);

#if ADVANCE_ALGORITHM
  if ((steps_e == 0) || (steps_x == 0 && steps_y == 0 && steps_z == 0)) {
    b->use_advance = false;
  } else {
    b->use_advance = true;
  }
  b->advance_multiplier = advance_get_multiplier(b);
  b->advance = advance_get_value(b);
#endif
}


/**
 * Creates and initializes a block to perform a linear movement and places this
 * block into the planning queue. The linear movement is specified by giving
 * the number of steps to move in each direction. This function blocks if the
 * planning queue is full.
 *
 * @param steps_x           the number of steps to take in the x direction (can
 *                          be negative)
 * @param steps_y           the number of steps to take in the y direction (can
 *                          be negative)
 * @param steps_z           the number of steps to take in the z direction (can
 *                          be negative)
 * @param steps_e           the number of steps to take in the e direction (can
 *                          be negative)
 * @param plateau_speed     maximum speed of the block in mm/min
 * @param collision_handler a pointer to a function to be called when one of 
 *                          the endstop is hit during execution of the block
 *
 * If the lookahead component is enabled, it is called immediately after adding
 * the new block to the queue, in order to optimize the block's entry speed and
 * to reduce the jerkiness of the transitions between blocks. The machine's
 * virtual position is updated to the position it will be in after this
 * movement has been executed.
 */
static void 
linear_movement_rel(steps_t steps_x, steps_t steps_y, steps_t steps_z, steps_t steps_e, speed_t plateau_speed, void (*collision_handler)(uint8_t))
{
  if (steps_x == 0 && steps_y == 0 && steps_z == 0 && steps_e == 0) {
    /* This is a null-move */
    return;
  }
  if (plateau_speed <= 0) {
    /* Invalid feed rate */
    return;
  }

  
  block_t *b = plan_q_head();
  while (b == NULL) {
    /* Wait until space is available in the planning queue */
    b = plan_q_head();
  } 
  
  // Fill in the variables required for the block handler
  initialize_block(b, steps_x, steps_y, steps_z, steps_e, plateau_speed, collision_handler);
  // Move planning queue head
  plan_q_shift_head();

#if PLANNER_LOOKAHEAD
  plan_lookahead();
#endif

  plan_set_virtual_position_rel(steps_x,steps_y, steps_z,steps_e);
}

/**
 * Function which is used as the collision handler for all normal movements. 
 * That is, movements that are not part of homing calibration.
 *
 * @param collisions a bit array of flags indicating which endstop was hit
 */
static void
collision(uint8_t collisions)
{
  block_handler_stop();
  plan_q_clear();
  // TODO: properly signal hardware fault
  puts("!! // collision on axis ");
  if (collisions & _BV(X_AXIS_POS)) {
    puts("X+ ");
  }
  if (collisions & _BV(X_AXIS_NEG)) {
    puts("X- ");
  }
  if (collisions & _BV(Y_AXIS_POS)) {
    puts("Y+ ");
  }
  if (collisions & _BV(Y_AXIS_NEG)) {
    puts("Y- ");
  }
  if (collisions & _BV(Z_AXIS_POS)) {
    puts("Z+ ");
  }
  if (collisions & _BV(Z_AXIS_NEG)) {
    puts("Z- ");
  }
  puts("while moving!\n");
}

/**
 * Function which is used as the collision handler for all homing movements. 
 *
 * @param collisions a bit array of flags indicating which endstop was hit
 */
static void
homing_collision(uint8_t collisions)
{
  return;
}



void
plan_init(void)
{
  plan_q_init();
#if PLANNER_LOOKAHEAD
  plan_lookahead_init();
#endif
  axis_homed = 0;
  virtual_position[E_AXIS] = 0;
  next_id = 0;
}

steps_t
plan_get_virtual_position(unsigned char axis)
{
  return virtual_position[axis];
}

void
plan_set_virtual_position_rel(steps_t x, steps_t y, steps_t z, steps_t e)
{
  virtual_position[X_AXIS] += x;
  virtual_position[Y_AXIS] += y;
  virtual_position[Z_AXIS] += z;
  virtual_position[E_AXIS] += e;
}

void
plan_set_virtual_position_abs(steps_t x, steps_t y, steps_t z, steps_t e)
{
  virtual_position[X_AXIS] = x;
  virtual_position[Y_AXIS] = y;
  virtual_position[Z_AXIS] = z;
  virtual_position[E_AXIS] = e;
}


void
plan_calculate_trapezoid(block_t *b, speed_t entry_speed, speed_t exit_speed)
{
  /* Calculate the acceleration variables */
  float common_multiplier = ((float)(b->nb_steps * 2))/(ACCELERATION * b->distance_mm);
  uint16_t entry_speed_step = (uint16_t)lround(lsquare(entry_speed) * common_multiplier) + 1;
  uint16_t exit_speed_step = (uint16_t)lround(lsquare(exit_speed) * common_multiplier) + 1;
  uint16_t plateau_reached_step = (uint16_t)lround(lsquare(b->plateau_speed) * common_multiplier) + 1;

  int32_t nb_acceleration_steps = (plateau_reached_step - entry_speed_step)/4;
  int32_t nb_deceleration_steps = (plateau_reached_step - exit_speed_step)/4;
  int32_t nb_plateau_steps = b->nb_steps - nb_acceleration_steps - nb_deceleration_steps;

  if (nb_plateau_steps < 0) {  
    nb_acceleration_steps = BOUNDS(0,intersection_steps(entry_speed,exit_speed,b->distance_mm,b->nb_steps),b->nb_steps);
    nb_plateau_steps = 0;
  }
  uint32_t deceleration_start = (uint32_t)(nb_acceleration_steps + nb_plateau_steps);
  fxp16u16_t entry_timer_ticks = ticks_float_to_fxp16u16_safe(b->mm_ticks_per_step_min/entry_speed); //rttimer ticks per step
  fxp16u16_t exit_timer_ticks = ticks_float_to_fxp16u16_safe(b->mm_ticks_per_step_min/exit_speed); //rttimer ticks per step


  /* Fill in the block acceleration variables if the block hasn't started yet */
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if (b->nb_steps_completed < deceleration_start && 
        b->nb_steps_completed < b->deceleration_start) {
      b->deceleration_start = deceleration_start;
      b->exit_timer_ticks = exit_timer_ticks; 
    }
    if (b->nb_steps_completed == 0) {
      b->acceleration_end = nb_acceleration_steps;
      b->timer_ticks = entry_timer_ticks;
      b->acceleration_step = entry_speed_step;
    }
  }
}

void 
plan_linear_movement_rel(steps_t steps_x, steps_t steps_y, steps_t steps_z, steps_t steps_e, speed_t plateau_speed)
{
  if (axis_homed != (_BV(X_AXIS)|_BV(Y_AXIS)|_BV(Z_AXIS))) {
    /* Home all axes before we make a linear movement */
    plan_home_axes(!(axis_homed & _BV(X_AXIS)),!(axis_homed & _BV(Y_AXIS)),!(axis_homed & _BV(Z_AXIS)));
  }

  if ((steps_x < -virtual_position[X_AXIS]) ||
      (steps_y < -virtual_position[Y_AXIS]) ||
      (steps_z < -virtual_position[Z_AXIS]) ||
      (steps_x > X_LENGTH*X_STEPS_PER_MM - virtual_position[X_AXIS]) ||
      (steps_y > Y_LENGTH*Y_STEPS_PER_MM - virtual_position[Y_AXIS]) ||
      (steps_z > Z_LENGTH*Z_STEPS_PER_MM - virtual_position[Z_AXIS])) {
    /* This is a request to move out of bounds */
    //TODO: signal error
    return;
  }

  linear_movement_rel(steps_x, steps_y, steps_z, steps_e, plateau_speed, collision);
}


void 
plan_linear_movement_abs(steps_t dest_x, steps_t dest_y, steps_t dest_z, steps_t dest_e, speed_t plateau_speed)
{
  plan_linear_movement_rel(
    dest_x - virtual_position[X_AXIS],
    dest_y - virtual_position[Y_AXIS],
    dest_z - virtual_position[Z_AXIS],
    dest_e - virtual_position[E_AXIS],
    plateau_speed
  );
}



void 
plan_home_axes(bool x, bool y, bool z)
{
  /* First move all axes to the origin fast, then move away from the origin fast
   * and finally return to the origin slowly for increased accuracy. */
  if (x) {
    linear_movement_rel((-2 * X_LENGTH * X_STEPS_PER_MM), 0, 0, 0, X_MAX_SPEED/2, homing_collision);
    linear_movement_rel((HOME_AXIS_CALIBRATION_MM_X * X_STEPS_PER_MM), 0, 0, 0, X_MAX_SPEED, collision);
    linear_movement_rel((-2 * HOME_AXIS_CALIBRATION_MM_X * X_STEPS_PER_MM), 0, 0, 0, HOME_AXIS_CALIBRATION_SPEED_X, homing_collision);
    virtual_position[X_AXIS] = 0;
    axis_homed |= _BV(X_AXIS);
  }
  if (y) {
    linear_movement_rel(0, (-2 * Y_LENGTH * Y_STEPS_PER_MM), 0, 0, Y_MAX_SPEED/2, homing_collision);
    linear_movement_rel(0, (HOME_AXIS_CALIBRATION_MM_Y * Y_STEPS_PER_MM), 0, 0, Y_MAX_SPEED, collision);
    linear_movement_rel(0, (-2 * HOME_AXIS_CALIBRATION_MM_Y * Y_STEPS_PER_MM), 0, 0, HOME_AXIS_CALIBRATION_SPEED_Y, homing_collision);
    virtual_position[Y_AXIS] = 0;
    axis_homed |= _BV(Y_AXIS);
  }
  if (z) {
    linear_movement_rel(0, 0, (-2 * Z_LENGTH * Z_STEPS_PER_MM), 0, Z_MAX_SPEED/2, homing_collision);
    linear_movement_rel(0, 0, (HOME_AXIS_CALIBRATION_MM_Z * Z_STEPS_PER_MM), 0, Z_MAX_SPEED, collision);
    linear_movement_rel(0, 0, (-2 * HOME_AXIS_CALIBRATION_MM_Z * Z_STEPS_PER_MM), 0, HOME_AXIS_CALIBRATION_SPEED_Z, homing_collision);
    virtual_position[Z_AXIS] = 0;
    axis_homed |= _BV(Z_AXIS);
  }
}


